// main.c
// Josh
// Jim 
// standard libs
#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>

#include "main.h"
#include "ff_utils.h"
#include "../utils/LCD_functions.h"
#include "../utils/util.h"
#include "../utils/button.h"

extern volatile char KEY_VALID;

// line sensors are connected to ADC4(PF4) thru ADC7(PF7).  Port F is also known as the JTAG port and 
// JTAG must be disabled inorder to use these pins.  The pins are not numbered the same as the other connectors.
//                              +---+---+---+---+---+
//            Port F  (JTAG)    | X | X | X | X | X |  Pins marked with an X should not be used.  
//            Usable Pins       +---+---+---+---+---+
//                              | 7 | X | 5 | 6 | 4 |  
//                              +---+---+---+---+---+


// Motor control circuit is connected to PortB  
//                              +---+---+---+---+---+
//            Port B            | 1 | 3 | X | X | V |  Pins marked with an X are not be used.  
//     Motor Contorl pins       +---+---+---+---+---+
//                              | 0 | 2 | x | x | G |  
//                              +---+---+---+---+---+
//  PB0 - Left motor PWM
//  PB1 - Right motor PWM
//  PB2 - Left motor Dir
//  PB3 - Right motor Dir
	

int main (void)
{ 
    Initialization();   //
	disable_JTAG();     // Disable JTAG port, making pins available for other use.
	I2C_Init();			// Inits I2C Routines
    SetupLineSensors();
    InitADC();
    show_mhz();			// Display CPU speed
    TimerWait(500);		// Wait 1/2 second
	LCD_puts_f(PSTR("FF v2.1.07"),1);
    TimerWait(2000);	// Wait 2 seconds
	unsigned char	OriginalHeading;
	OriginalHeading = 0x7A;
    //
	while(1)
	{
		LCD_puts_f(PSTR("R wall follow"),1);
        while (!KEY_VALID);
        if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
        {
            TimerWait(500);		// delay while letting go!
            RWallFollow(0x30,UNTIL_KEYPRESS,1);
            LCD_puts_f(PSTR("Back"),1);
            TimerWait(1000);	
		}	
		LCD_puts_f(PSTR("L wall follow"),1);
        while (!KEY_VALID);
        if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
        {
            TimerWait(500);		// delay while letting go!
            LWallFollow(0x30,UNTIL_KEYPRESS,1);	
            LCD_puts_f(PSTR("Back"),1);
            TimerWait(1000);	
		}		
		LCD_puts_f(PSTR("Home F 1"),1);
        while (!KEY_VALID);
        if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
        {
            TimerWait(500);		// delay while letting go!
            HomeFrom1();
            LCD_puts_f(PSTR("Back"),1);
            TimerWait(500);
		}			
		LCD_puts_f(PSTR("Home F 2"),1);
        while (!KEY_VALID);
        if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
        {
            TimerWait(500);		// delay while letting go!
            HomeFrom2();
            LCD_puts_f(PSTR("Back"),1);
            TimerWait(500);
		}			
		LCD_puts_f(PSTR("Home F 3"),1);
        while (!KEY_VALID);
        if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
        {
            TimerWait(500);		// delay while letting go!
            HomeFrom3();
            LCD_puts_f(PSTR("Back"),1);
            TimerWait(500);
		}			
		LCD_puts_f(PSTR("Home F 4"),1);
        while (!KEY_VALID);
        if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
        {
            TimerWait(500);		// delay while letting go!
            HomeFrom4();
            LCD_puts_f(PSTR("Back"),1);
            TimerWait(500);
		}				
		LCD_puts_f(PSTR("Exit Room"),1);
        while (!KEY_VALID);
        if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
        {
            TimerWait(500);		// delay while letting go!
            ExitRoom();
            LCD_puts_f(PSTR("Back"),1);
            TimerWait(500);
		}		
		LCD_puts_f(PSTR("2Room 3"),1);
        while (!KEY_VALID);
        if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
        {
            TimerWait(500);		// delay while letting go!
            ToRoom3();
            LCD_puts_f(PSTR("Back"),1);
            TimerWait(500);
        }
		LCD_puts_f(PSTR("Room 4"),1);
        while (!KEY_VALID);
        if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
        {
            TimerWait(500);		// delay while letting go!
            ToRoom4();
            LCD_puts_f(PSTR("Back"),1);
            TimerWait(500);
		}	
		LCD_puts_f(PSTR(" 1 Room 2"),1);
        while (!KEY_VALID);
        if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
        {
            TimerWait(500);		// delay while letting go!
            ToRoom2();
            LCD_puts_f(PSTR("Back"),1);
            TimerWait(500);
        }
		
		
		LCD_puts_f(PSTR("Compass Diff"),1);
        while (!KEY_VALID);
        if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
        {
            TimerWait(500);		// delay while letting go!
            CompassDiff();
            LCD_puts_f(PSTR("Back"),1);
            TimerWait(500);
        }
		//while (KEY_VALID == FALSE); // The Joystick returns FALSE when not pressed. 
        //TimerWait(500);				// delay while letting go!
		LCD_puts_f(PSTR("Check For Flame"),1);
        while (!KEY_VALID);
        if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
        {
            TimerWait(500);		// delay while letting go!
            CheckForFlame();
            LCD_puts_f(PSTR("Back"),1);
            TimerWait(500);
        }
		LCD_puts_f(PSTR("To Room 1"),1);
        while (!KEY_VALID);
        if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
        {
            TimerWait(500);		// delay while letting go!
            ToRoom1();
            LCD_puts_f(PSTR("Back"),1);
            TimerWait(500);
        }			
		LCD_puts_f(PSTR("Go To Heading Test"),1);
        while (!KEY_VALID);
        if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
        {
            TimerWait(500);		// delay while letting go!
            HeadingTest();
            LCD_puts_f(PSTR("Back"),1);
            TimerWait(1000);
        }					
		LCD_puts_f(PSTR("Therm Noise Test"),1);
        while (!KEY_VALID);
        if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
        {
            TimerWait(500);		// delay while letting go!
            ThermNoise();
            LCD_puts_f(PSTR("Back"),1);
        }
		LCD_puts_f(PSTR("Comp Noise Test"),1);
        while (!KEY_VALID);
        if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
        {
            TimerWait(500);		// delay while letting go!
            CompNoise();
            LCD_puts_f(PSTR("Back"),1);
        }		
		LCD_puts_f(PSTR("Center Room Test"),1);
        while (!KEY_VALID);
        if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
        {
            TimerWait(500);		// delay while letting go!
            CenterRoomFlame();
            LCD_puts_f(PSTR("Back"),1);
            TimerWait(1500);
        }
	
		LCD_puts_f(PSTR("Go To Flame"),1);
        while (!KEY_VALID);
        if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
        {
            TimerWait(500);		// delay while letting go!
            GoToFlame();
            LCD_puts_f(PSTR("Back"),1);
            TimerWait(1500);
        }

		LCD_puts_f(PSTR("Flame Sensor Test"),1);
        while (!KEY_VALID);
        if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
        {
            TimerWait(500);		// delay while letting go!
            FlameSensorTest();
            LCD_puts_f(PSTR("Back"),1);
            TimerWait(1000);
        }

		LCD_puts_f(PSTR("Compass Test"),1);
        while (!KEY_VALID);
        if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
        {
            TimerWait(500);		// delay while letting go!
            CompassTest();
            LCD_puts_f(PSTR("Back"),1);
            TimerWait(1000);
        }
		
		LCD_puts_f(PSTR("L R DistTest"),1);
        while (!KEY_VALID);
        if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
        {
            TimerWait(500);		// delay while letting go!
            showADCLeftRight();
            LCD_puts_f(PSTR("Back"),1);
            TimerWait(1000);
        }
		
		LCD_puts_f(PSTR("C DistTest"),1);
        while (!KEY_VALID);
        if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
        {
            TimerWait(500);		// delay while letting go!
            showADCCenter();
            LCD_puts_f(PSTR("Back"),1);
            TimerWait(1000);
		}
		LCD_puts_f(PSTR("Forward Test"),1);
        while (!KEY_VALID);
        if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
        {
            TimerWait(500);		// delay while letting go!
			Forward(10,UNTIL_TIME,TimerMsCur() + 4000);
            LCD_puts_f(PSTR("Back"),1);
            TimerWait(1000);	
		}		
		LCD_puts_f(PSTR("Check for flame"),1);
		while (!KEY_VALID);
        if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
        {
            TimerWait(500);		// delay while letting go!
            CheckForFlame();
            LCD_puts_f(PSTR("Back"),1);
            TimerWait(1500);
        }	
		LCD_puts_f(PSTR("PWM Test"),1);
		while (!KEY_VALID);
        if (getkey() == 1)                      // If enter was pressed then do what is in the braces, just skip over it. 
        {
            TimerWait(500);		// delay while letting go!
            PWM_Test();
            LCD_puts_f(PSTR("Back"),1);
            TimerWait(1500);
        }	
	}
}	
	
 void HomeFrom1(void)
{
	Forward(-8,UNTIL_TIME,TimerMsCur() + 500);
	LCD_puts_f(PSTR("Exiting"),1);		
	ExitRoom();
	GoToHeading((RoomOneEnterHeading +128),8);	
	TimerWait(1000);
	Forward(10,UNTIL_TIME,TimerMsCur() + 500);	
	RWallFollow(0x30,UNTIL_TIME,TimerMsCur() + 2000);//	
	LCD_puts_f(PSTR("2500"),1);		
	LWallFollow(0x30,UNTIL_LINESENSOR_LOWER,0x40);// line sensor   when left sees doorway 
	TimerWait(100);
	Forward(8,UNTIL_TIME,TimerMsCur() + 500); 
	TimerWait(10000);
}

 void HomeFrom2(void)
{
	Forward(-8,UNTIL_TIME,TimerMsCur() + 500);
	LCD_puts_f(PSTR("Exiting"),1);		
	ExitRoom();
	GoToHeading((RoomTwoEnterHeading +128),8);	
	TimerWait(1000);
	Forward(10,UNTIL_TIME,TimerMsCur() + 500);	
	LWallFollow(0x30,UNTIL_LINESENSOR_LOWER,0x40);// line sensor   when left sees doorway 
	TimerWait(100);
	Forward(8,UNTIL_TIME,TimerMsCur() + 500); 
	TimerWait(10000);
}

 void HomeFrom3(void)
{
	Forward(-8,UNTIL_TIME,TimerMsCur() + 500);
	LCD_puts_f(PSTR("Exiting"),1);		
	ExitRoom();
	GoToHeading((RoomThreeEnterHeading +128),8);	
	TimerWait(1000);
	Forward(10,UNTIL_TIME,TimerMsCur() + 500);	
	RWallFollow(0x30,UNTIL_LINESENSOR_LOWER,0x40);// line sensor   when left sees doorway 
	GoToHeading((RoomFourEnterHeading +150),8);
	Forward(10,UNTIL_TIME,TimerMsCur() + 800);
	RWallFollow(0x30,UNTIL_LINESENSOR_LOWER,0x40);	
	TimerWait(100);
	Forward(8,UNTIL_TIME,TimerMsCur() + 500); 
	TimerWait(10000);
}
 void HomeFrom4(void)
{
	Forward(-8,UNTIL_TIME,TimerMsCur() + 500);
	LCD_puts_f(PSTR("Exiting"),1);		
	ExitRoom();
	GoToHeading((RoomFourEnterHeading +136),8);	
	TimerWait(1000);
	Forward(10,UNTIL_TIME,TimerMsCur() + 700);	
	RWallFollow(0x30,UNTIL_LINESENSOR_LOWER,0x40);// line sensor   when left sees doorway 
	Forward(10,UNTIL_TIME,TimerMsCur() + 500);
	TimerWait(100);
	Forward(8,UNTIL_TIME,TimerMsCur() + 500); 
	TimerWait(10000);
}
void ToRoom1(void)
{
	Forward(10,UNTIL_TIME,TimerMsCur() + 1000);
	LWallFollow(0x30,UNTIL_LEFTSENSOR_LOWER,0x20);
	LWallFollow(0x24,UNTIL_LINESENSOR_LOWER,0x40);// line sensor   when left sees doorway 
	GoToHeading((RoomOneEnterHeading + 8), 8); //132
	LCD_puts_f(PSTR("SQUARE"),1);
	TimerWait(100);
	Forward(8,UNTIL_TIME,TimerMsCur() + 1000);	
	LCD_puts_f(PSTR("SCAN ROOM"),1);
	TimerWait(100);

	if(CheckForFlame())
	{	
		GoToFlame();
		HomeFrom1();
	}
	else
	{
		LCD_puts_f(PSTR("Back from Check for flame"),1);	
		TimerWait(1000);
		GoToHeading((RoomOneEnterHeading + 112 ), 8);
		LCD_puts_f(PSTR("Org Heading"),1);
		TimerWait(1000);		
		Forward(8,UNTIL_LINESENSOR_LOWER,0x40);
		// goes To Room 2 from room 1 door white line
		ToRoom2();
	}
	LCD_puts_f(PSTR("Exit"),1);
}


void ToRoom2(void)//go to room 2 from room 1
{
	Forward(8,UNTIL_TIME,TimerMsCur() + 700);
	RWallFollow(0x20,UNTIL_LINESENSOR_LOWER,0x40);// follows right wall untill line sensor sees doorway 
	GoToHeading((RoomTwoEnterHeading - 8), 7);
	Forward(8,UNTIL_TIME,TimerMsCur() + 1000);
	LCD_puts_f(PSTR("SQUARE"),1);
	TimerWait(1000);	
	LCD_puts_f(PSTR("SCAN ROOM"),1);
	TimerWait(1000);

	if(CheckForFlame())
	{
		GoToFlame();
		HomeFrom2();
/*		unsigned char	FlameHeading	= Heading;

		GoToFlame();
		/// *** works? check or replace with home from 2
		Forward(-8,UNTIL_TIME,TimerMsCur() + 1000);
		TimerWait(2000);
		GoToHeading(FlameHeading + 128, 8);
		TimerWait(2000);
		LWallFollow(0x30,UNTIL_LINESENSOR_LOWER,0x40);// line sensor   when left sees doorway 
		TimerWait(2000);
		GoToHeading(OriginalHeading + 32, 8);
		TimerWait(2000);
		Forward(8,UNTIL_LEFTSENSOR_GREATER,0x30);
		TimerWait(2000);
		LWallFollow(0x30,UNTIL_LINESENSOR_LOWER,0x40);// line sensor   when left sees doorway
		TimerWait(2000);
		Forward(8,UNTIL_TIME,TimerMsCur() + 500); 
		TimerWait(2000);
		LWallFollow(0x30,UNTIL_LINESENSOR_LOWER,0x40);// line sensor   when left sees doorway 
		//Go Home From Room 1
		TimerWait(2000);*/
	}
	else
	{
		LCD_puts_f(PSTR("Back from Check for flame"),1);	
		TimerWait(500);
		GoToHeading((RoomTwoEnterHeading+ 132), 7);
		TimerWait(500);		
		Forward(8,UNTIL_LINESENSOR_LOWER,0x40);
		//LCD_puts_f(PSTR("Done"),1);
		//TimerWait(1000);
		ToRoom3();
	}
	LCD_puts_f(PSTR("Exit"),1);
}		

void ToRoom3(void)//go to room 3 from room 2
{
    Forward(8,UNTIL_TIME,TimerMsCur() + 500);
	RWallFollow(0x20,UNTIL_LINESENSOR_LOWER,0x40);
	TimerWait(1000);
    Forward(8,UNTIL_TIME,TimerMsCur() + 100);	
	GoToHeading((RoomThreeEnterHeading), 8);
	// *** change to lower angle to center in doorway
	//TimerWait(1000);
	Forward(8, UNTIL_MIDDLESENSOR_GREATER,0x25);
	if (CheckForFlame())
	{
		GoToFlame();
		HomeFrom3(); 
	}
	else
	{
		GoToHeading((RoomThreeEnterHeading +136), 8);
		//TimerWait(1000);
		Forward(8, UNTIL_LINESENSOR_LOWER, 0x40);// maybe follow right wall ***************
		//LCD_puts_f(PSTR("EXIT R3"),1);
		//TimerWait(1000);
		ToRoom4();
	}
	LCD_puts_f(PSTR("Exit"),1);

}

void ToRoom4(void)//go to room 4 from room 3

{
	GoToHeading((RoomThreeEnterHeading -120), 8);
	Forward(8, UNTIL_TIME, TimerMsCur() + 200);
	RWallFollow(0x37, UNTIL_LINESENSOR_LOWER, 0x40);
	GoToHeading((RoomFourEnterHeading - 8), 8);	
	Forward(8, UNTIL_TIME, TimerMsCur() + 1000);
	if (CheckForFlame())
	{
		GoToFlame();
		HomeFrom4();
	}
	else
	{
	    GoToHeading((RoomFourEnterHeading+128), 8);	
		Forward(8, UNTIL_LINESENSOR_LOWER, 0x40);	
		Forward(8, UNTIL_TIME, TimerMsCur() + 100);	
		RWallFollow(0x30,UNTIL_LINESENSOR_LOWER,0x40);// line sensor   when left sees doorway 
		TimerWait(100);
		Forward(8,UNTIL_TIME,TimerMsCur() + 500); 
		TimerWait(100);
	}
	LCD_puts_f(PSTR("Exit"),1);	
}	



void Forward(int Speed,int TerminatingCondition,unsigned long TerminatingValue)
{
	do
	{
		SetLeftMotorPWM(Speed);
		SetRightMotorPWM(Speed);
	} while(!CheckTerminatingConditions(TerminatingCondition,TerminatingValue));

	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
}


void RWallFollow(int TargetDistance,int TerminatingCondition,unsigned long TerminatingValue)
{
	unsigned char adcL, adcM, adcR;
   
	do
	{
		adcR = GetADC(RightDistSensor);
		adcM = GetADC(MiddleDistSensor);
		adcL = GetADC(LeftDistSensor);
		
		if(adcM > 0x2A)
		{
			SetLeftMotorPWM(3);
			SetRightMotorPWM(10);
		}			
//		else if(adcR > (TargetDistance + 10))
//		{
//			SetLeftMotorPWM(4);
//			SetRightMotorPWM(10);
//		}	
		else if(adcR > (TargetDistance + 8))
		{
			SetLeftMotorPWM(6);
			SetRightMotorPWM(10);
		}
		else if(adcR > (TargetDistance + 6))
		{
			SetLeftMotorPWM(8);
			SetRightMotorPWM(10);
		}		
		else if(adcR > (TargetDistance + 4))
		{
			SetLeftMotorPWM(9);
			SetRightMotorPWM(10);
		}
		else if(adcR > (TargetDistance + 2))
		{
			SetLeftMotorPWM(10);
			SetRightMotorPWM(10);
		}	
//		else if(adcR < (TargetDistance - 10))
//		{
//			SetLeftMotorPWM(10);
//			SetRightMotorPWM(4);	
//		}			
		else if(adcR < (TargetDistance - 8))
		{
			SetLeftMotorPWM(10);
			SetRightMotorPWM(6);	
		}	
		else if(adcR < (TargetDistance - 6))
		{
			SetLeftMotorPWM(10);
			SetRightMotorPWM(8);	
		}
		else if(adcR < (TargetDistance - 4))
		{
			SetLeftMotorPWM(10);
			SetRightMotorPWM(9);
		}		
		else if(adcR < (TargetDistance - 2))
		{
			SetLeftMotorPWM(10);
			SetRightMotorPWM(10);
		}
		else
		{
			SetLeftMotorPWM(10);
			SetRightMotorPWM(10);
		}
		


	} while(!CheckTerminatingConditions(TerminatingCondition,TerminatingValue));
	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
}

void LWallFollow(int TargetDistance,int TerminatingCondition,unsigned long TerminatingValue)
{
	unsigned char adcL, adcM, adcR;
   
	do
	{
		adcR = GetADC(RightDistSensor);
		adcM = GetADC(MiddleDistSensor);
		adcL = GetADC(LeftDistSensor);
		//show2(adcL, adcM);
		sendNumber(adcL);
		sendChar(',');
		sendNumber(adcM);
		sendChar(',');
		sendNumber(adcR);
		sendString("\r\n");
		if(adcM > 0x25)
		{
			SetLeftMotorPWM(10);
			SetRightMotorPWM(3);
		}
//		if(adcL < (TargetDistance - 10))
//		{
//			SetLeftMotorPWM(4);
//			SetRightMotorPWM(10);
//		}		
		else if(adcL < (TargetDistance - 8))
		{
			SetLeftMotorPWM(6);
			SetRightMotorPWM(10);
		}				
		else if(adcL < (TargetDistance - 6))
		{
			SetLeftMotorPWM(8);
			SetRightMotorPWM(10);
		}		
		else if(adcL < (TargetDistance - 4))
		{
			SetLeftMotorPWM(9);
			SetRightMotorPWM(10);
		}
		else if(adcL < (TargetDistance - 2))
		{
			SetLeftMotorPWM(10);
			SetRightMotorPWM(10);
		}
//		else if(adcL > (TargetDistance + 10))
//		{
//			SetLeftMotorPWM(10);
//			SetRightMotorPWM(4);
//		}
		else if(adcL > (TargetDistance + 8))
		{
			SetLeftMotorPWM(10);
			SetRightMotorPWM(6);
		}		
		else if(adcL > (TargetDistance + 6))
		{
			SetLeftMotorPWM(10);
			SetRightMotorPWM(8);
		}
		else if(adcL > (TargetDistance + 4))
		{
			SetLeftMotorPWM(10);
			SetRightMotorPWM(9);
		}
		else if(adcL > (TargetDistance + 2))
		{
			SetLeftMotorPWM(10);
			SetRightMotorPWM(10);
		}		
		else
		{
			SetLeftMotorPWM(10);
			SetRightMotorPWM(10);
		}

	} while(!CheckTerminatingConditions(TerminatingCondition,TerminatingValue));
	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
}
	
/*void LWallFollow(void)
{
	
	unsigned char adcL, adcM, adcR;
   
	while (!(getkey() == 1))
	{
		//while (adcM < 0X30){
		adcR = GetADC(RightDistSensor);
		adcM = GetADC(MiddleDistSensor);
		adcL = GetADC(LeftDistSensor);
		show2(adcL, adcM);
		if (adcL < 0x30)//(-10 < (Heading-TargetHeading) && 10 > (Heading-TargetHeading))
		{
			SetLeftMotorPWM(8);
			SetRightMotorPWM(0);
		}
		else 
		{
			SetLeftMotorPWM(0);
			SetRightMotorPWM(8);
		}
	}	
}*/

//this is a test
